/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Behaviors.Schemas.Perception;

import Behaviors.Schemas.*;
import Utils.*;
import Robot.Sensors.*;
import Robot.*;
/**
 *
 * @author alejo
 */
public class PSMapVP extends Schema implements Vector2dSchema{

    private VPSensor vp;
    private Position2d myPos;
    private int centerImgX;
    private int centerImgY;
    private int msgNum = 0;
    private int fidelity = 0;


    public PSMapVP(String name, VPSensor vp, Position2d myPos)
    {
        super(name);
        this.vp = vp;
        this.centerImgX = vp.getWidth()/2;
        this.centerImgY = vp.getHeight()/2;
        this.myPos = myPos;

    }

    public int getFidelity()
    {
        return this.fidelity;
    }

    public Vector2d Value()
    {
        int vpX = vp.getVpX();
        fidelity = vp.getVpFid();
        int actMsgNum = vp.getMsgNum();

        //only calculate new point if a vanishPoint really was calculated.
        //when the vanishPoint was not calculated, then the system returns -1
        if(vpX >= 0 && actMsgNum != msgNum )
        {
            msgNum = actMsgNum;
            //x value of image is not taken into account.
            int varX = centerImgX - vp.getVpX();


            System.out.println("x: "+vp.getVpX()+" centerImg: "+centerImgX+" varX: "+varX);

            //variable x in image really represents y positioning in robot
            float y = (float)((float)varX / (float)(centerImgX));

            if(y > 1)
                y = 1;
            else if(y < -1)
                y = -1;

            float x = (float)Math.sqrt(4-y*y);

            Pose2d pos = myPos.read(Position2d.READ_MODE_NO_WAIT);
            float angle = (float)pos.getPa();

            Vector2d result = new Vector2d(x,y,Vector2d.MODE_XY);
            result.setMag(1);

//            System.out.println("obstaculo referenciado al robot");
//            result.print(Vector2d.MODE_XY);

            //float newAng = Vector2d.angleCorrMPI2PI((float)(result.getAngle()-Math.PI/2+angle));
            result = result.rotate(angle);
//            System.out.println("obstaculo rotado con angle: "+angle);
//            result.print(Vector2d.MODE_XY);

            //result.setAngle(newAng);
            result = result.add(myPos.getPosition());
//            System.out.println("Posicion Robot:");
            myPos.getPosition().print(Vector2d.MODE_XY);



            return result;
        }
        else
            return null;
    }
}
